#include "planer.h"
#include "dijkstra.h"

std::vector<Path> Planer::makePlan(Point start, std::vector<Point> goals) {
    std::vector<int>              plan = map_->GreedyPlan(start, goals);
    std::vector<Path>             paths;
    std::vector<std::vector<int>> grid = map_->GetGrid();
    // 使用Dijkstra算法规划从起点到第一个目标点的路径
    paths.emplace_back(DijkstraPlan(grid, start, goals[plan[0]]));

    // 遍历计划中的所有目标点，使用Dijkstra算法规划相邻目标点之间的路径
    for (int i = 0; i < plan.size() - 1; i++) {
        Point start = goals[plan[i]];
        Point goal  = goals[plan[i + 1]];
        Path  path  = DijkstraPlan(grid, start, goal);
        if (path.empty()) {
            std::cerr << "No path found from:" << start << " to " << goal << std::endl;
            continue;
        }

        paths.emplace_back(path);
    }
    return paths;
}

std::vector<Path> Planer::makePlan(Point start) {
    std::vector<std::vector<int>> grid = map_->GetGrid();

    std::vector<Point> goals;
    // 遍历地图的每一列
    for (int x = 0; x < grid[0].size(); x++) {
        // 偶数列，从上到下添加点
        if (x % 2 == 0) {
            for (int y = 0; y < grid.size(); y++) {
                if (grid[y][x] == kObstacle)
                    continue;

                goals.emplace_back(x, y);
            }
        }  // 奇数列，从上到下添加点
        else {
            for (int y = grid.size() - 1; y >= 0; y--) {
                if (grid[y][x] == kObstacle)
                    continue;
                goals.emplace_back(x, y);
            }
        }
    }

    std::vector<Path> paths;
    // 遍历所有目标点，使用Dijkstra算法规划相邻目标点之间的路径
    for (int i = 0; i < goals.size() - 1; i++) {
        // 第一个目标点，需要先添加起到到该目标点的路径
        if (i == 0) {
            Path path = DijkstraPlan(grid, start, goals[0]);
            paths.emplace_back(path);
        }
        Point start = goals[i];
        Point goal  = goals[i + 1];
        Path  path  = DijkstraPlan(grid, start, goal);
        if (path.empty()) {
            std::cerr << "No path found from:" << start << " to " << goal << std::endl;
            continue;
        }

        paths.emplace_back(path);
    }

    return paths;
}

// 使用贪婪算法生成从起点到多个目标点的路径计划
std::vector<int> Planer::GreedyPlan(Point start, std::vector<Point> goals) {
    // 使用贪心算法获取目标点集的最优排序
    std::vector<int> plan = map_->GreedyPlan(start, goals);

    return plan;
}

// 模拟退火算法生成从起点到多个目标点的路径
// TODO:暂时还未实现
std::vector<int> Planer::SAPlan(Point start, std::vector<Point> goals) {
    return {};
}

// 创建并返回一个Planer实例的指针
PlanInterface* PlanInterface::create(std::vector<std::vector<int>>& grid) {
    // 根据传入的网格和交叉行信息新建一个Planer对象
    return new Planer(grid);
}
